#ifndef _PSP_H_
#define _PSP_H_

#include <openssl/crypto.h>
#include <stdio.h>
#define PACKED __attribute__((packed))

typedef enum { /* return codes */
  SUCCESS_RC,
  ERR_RC
} rc_t;

#define DEFAULT_ENCRYPT_CFG_FILE "./cfg/psp_encrypt.cfg"
#define DEFAULT_DECRYPT_CFG_FILE "./cfg/psp_decrypt.cfg"

#define MAX_PCAP_CAPTURE_OCTETS 65535

#define ETH_MIN_OCTETS 64
#define ETH_MAX_OCTETS 1514
#define ETH_JUMBO_MAX_OCTETS 9014

#define MAC_ADDR_OCTETS 6
#define IPV4_ETYPE 0x0800
#define IPV6_ETYPE 0x86DD

#define IPV4_VER_IHL 0x45
#define IPV4_IHL_MASK 0x0f
#define IPV4_IHL_UNITS 4 /* units of 4 octets */
#define IPV4_IHL_NO_OPTIONS 5
#define IPV4_HDR_OCTETS_NO_OPTIONS (IPV4_IHL_NO_OPTIONS * IPV4_IHL_UNITS)
#define IPV4_FLAGS_DF 0x4000
#define IPV4_FLAGS_MF 0x2000
#define IP_TTL_DEF 0x40

#define IPV6_VER 0x60000000
#define IPV6_ADDR_OCTETS 16

#define IP_PROTO_IPV4 0x04
#define IP_PROTO_IPV6 0x29
#define IP_PROTO_UDP 0x11
#define IP_PROTO_TCP 0x06

#define UDP_PORT_PSP 1000

#define TCP_DATA_OFFSET 0x5000 /* default */
#define TCP_FLAG_NS 0x0100
#define TCP_FLAG_CWR 0x0080
#define TCP_FLAG_ECE 0x0040
#define TCP_FLAG_URG 0x0020
#define TCP_FLAG_ACK 0x0010
#define TCP_FLAG_PSH 0x0008
#define TCP_FLAG_RST 0x0004
#define TCP_FLAG_SYN 0x0002
#define TCP_FLAG_FIN 0x0001

typedef enum { PSP_TRANSPORT, PSP_TUNNEL } psp_encap_t;

typedef enum { AES_GCM_128, AES_GCM_256 } crypto_alg_t;

#define PSP_HDR_EXT_LEN_UNITS 8 /* units of 8 octets */
#define PSP_HDR_EXT_LEN_MIN 1
#define PSP_HDR_EXT_LEN_WITH_VC 2
#define PSP_HDR_VC_OCTETS 8

/* values to expose L4 port numbers, units of 4 octets */
#define PSP_CRYPT_OFFSET_UNITS 4 /* units of 4 octets */
#define PSP_CRYPT_OFFSET_MASK 0x3f
#define PSP_CRYPT_OFFSET_MAX 64
#define PSP_CRYPT_OFFSET_RESERVED_BIT7 0x80
#define PSP_CRYPT_OFFSET_RESERVED_BIT6 0x40
#define PSP_CRYPT_OFFSET_V4_TUNNEL 6
#define PSP_CRYPT_OFFSET_V6_TUNNEL 11

typedef enum {
  PSP_VER0 = 0, /* AES-GCM-128 */
  PSP_VER1,     /* AES-GCM-256 */
  PSP_VER2,     /* AES-GMAC-128 */
  PSP_VER3      /* AES-GMAC-256 */
} psp_ver_t;

#define PSP_HDR_FLAG_S_SHIFT 7 /* sample bit */
#define PSP_HDR_FLAG_D_SHIFT 6 /* drop bit */
#define PSP_HDR_VER_SHIFT 2    /* version bits */
#define PSP_HDR_VER_MASK 0x0f
#define PSP_HDR_FLAG_V_SHIFT 1 /* virtualization-cookie-present bit */
#define PSP_HDR_FLAG_V (1 << PSP_HDR_FLAG_V_SHIFT)
#define PSP_HDR_ALWAYS_1 1

#define PSP_SPI_OCTETS 4
#define PSP_SPI_KEY_SELECTOR_BIT 0x80000000 /* for uint32_t compare */
#define PSP_SPI_MSB_KEY_SELECTOR_BIT 0x80   /* for uint8_t compare */

#define PSP_INITIAL_IV 1 /* starting value for psp initialization vector */
#define PSP_IV_OCTETS 8
#define AES_GCM_IV_OCTETS 12

#define AES_128_KEY_OCTETS 16
#define AES_256_KEY_OCTETS 32

#define PSP_MASTER_KEY_OCTETS AES_256_KEY_OCTETS
#define PSP_DERIVED_KEY_MAX_OCTETS AES_256_KEY_OCTETS
#define PSP_KEY_DERIVATION_BLOCK_OCTETS 16
#define PSP_ICV_OCTETS 16

#define PSP_MODE_DECRYPT 1 
#define PSP_MODE_ENCRYPT 0
#define PKT_DECRYPTED 0
#define SEND_NUMBER 1024

struct eth_hdr {
  char dmac[MAC_ADDR_OCTETS];
  char smac[MAC_ADDR_OCTETS];
  uint16_t etype;
} PACKED;

struct ipv4_hdr {
  uint8_t ver_ihl;
  uint8_t tos;
  uint16_t len;
  uint16_t id;
  uint16_t flags_offset;
  uint8_t ttl;
  uint8_t proto;
  uint16_t csum;
  uint32_t sip;
  uint32_t dip;
};

struct ipv6_hdr {
  uint32_t ver_tc_flow;
  uint16_t plen;
  uint8_t proto;
  uint8_t ttl;
  uint8_t sip[IPV6_ADDR_OCTETS];
  uint8_t dip[IPV6_ADDR_OCTETS];
};

struct udp_hdr {
  uint16_t sport;
  uint16_t dport;
  uint16_t len;
  uint16_t csum;
};

struct tcp_hdr {
  uint16_t sport;
  uint16_t dport;
  uint32_t seq;
  uint32_t ack;
  uint16_t off_flags;
  uint16_t win;
  uint16_t csum;
  uint16_t urp;
};

struct psp_hdr {
  uint8_t next_hdr;
  uint8_t hdr_ext_len;
  uint8_t crypt_off;
  uint8_t s_d_ver_v_1;
  uint32_t spi;
  uint64_t iv;
};

struct psp_icv {
  uint8_t octets[PSP_ICV_OCTETS];
};

struct psp_trailer {
  struct psp_icv icv;
};

struct psp_v4_hdrs {
  struct eth_hdr eth;
  struct ipv4_hdr ipv4;
  struct udp_hdr udp;
  struct psp_hdr psp;
} PACKED;

struct psp_v6_hdrs {
  struct eth_hdr eth;
  struct ipv6_hdr ipv6;
  struct udp_hdr udp;
  struct psp_hdr psp;
} PACKED;

#define PSP_TRANSPORT_ENCAP_OCTETS \
  (sizeof(struct udp_hdr) + sizeof(struct psp_hdr) + sizeof(struct psp_trailer))

#define PSP_V4_TUNNEL_ENCAP_OCTETS \
  (PSP_TRANSPORT_ENCAP_OCTETS + sizeof(struct ipv4_hdr))

#define PSP_V6_TUNNEL_ENCAP_OCTETS \
  (PSP_TRANSPORT_ENCAP_OCTETS + sizeof(struct ipv6_hdr))

struct psp_master_key {
  uint8_t octets[PSP_MASTER_KEY_OCTETS];
};

struct psp_key_derivation_block {
  uint8_t octets[PSP_KEY_DERIVATION_BLOCK_OCTETS];
};

struct psp_derived_key {
  uint8_t octets[PSP_DERIVED_KEY_MAX_OCTETS];
};

struct aes_gcm_iv {
  uint8_t octets[AES_GCM_IV_OCTETS];
};

/* host-to-network and network-to-host for 64b */
#define HTONLL(x)                                                   \
  ((1 == htonl(1)) ? (x)                                            \
                   : ((((uint64_t)htonl((x)&0xFFFFFFFFUL)) << 32) | \
                      htonl((uint32_t)((x) >> 32))))
#define NTOHLL(x)                                                   \
  ((1 == ntohl(1)) ? (x)                                            \
                   : ((((uint64_t)ntohl((x)&0xFFFFFFFFUL)) << 32) | \
                      ntohl((uint32_t)((x) >> 32))))



/* compute IPv4 header checksum */
static inline uint16_t ipv4_hdr_csum(struct ipv4_hdr *h) {
  long sum = 0;
  uint16_t *p = (uint16_t *)h;
  int i, ihl_octets;

  ihl_octets = (h->ver_ihl & IPV4_IHL_MASK) * IPV4_IHL_UNITS;
  for (i = 0, sum = 0; i < (ihl_octets / 2); i++) sum += p[i];

  while (sum >> 16) sum = (sum & 0xffff) + (sum >> 16);

  return ((uint16_t)~sum);
}

typedef enum {                /* return codes for packet processsing */
               PKT_ENCRYPTED, /* success */
               PKT_SKIPPED,   /* packet not encrypted */
               PKT_ERR
} pkt_rc_t;

struct psp_encrypt_cfg { /* encryption config parms */
  struct psp_master_key master_key0;
  struct psp_master_key master_key1;
  uint32_t spi;
  psp_encap_t psp_encap;
  crypto_alg_t crypto_alg;
  /* crypt offset for transport mode, units = 4B */
  uint8_t transport_crypt_off;
  /* crypt offset for ipv4 packets in tunnel mode, units = 4B */
  uint8_t ipv4_tunnel_crypt_off;
  /* crypt offset for ipv6 packets in tunnel mode, units = 4B */
  uint8_t ipv6_tunnel_crypt_off;
  bool include_vc; /* include vc in psp header */
};

struct pkt_context {
    uint32_t max_pkt_octets;
    struct psp_encrypt_cfg psp_cfg;
    struct psp_derived_key key;
    uint64_t next_iv;
    uint8_t *in_pkt;
    uint8_t *out_pkt;
    uint8_t *scratch_buf;
    int psp_payload_len;
};

/*
 * derive 128b of psp encryption key
 *
 * parms:
 *   pkt_ctx: ptr to pcaket context struct
 *   counter: 1 => derive first 128b of key
 *            2 => derive second 128b of key
 *   derived_key: ptr to location where derived ket is returned
 *
 * returns:
 *   SUCCESS_RC: key derived successfully
 *   ERR_RC:     error deriving key
 */
static rc_t derive_psp_key_128(struct pkt_context *pkt_ctx, uint8_t counter,
                               uint8_t *derived_key) {
  CMAC_CTX *ctx = NULL;
  uint32_t spi;
  struct psp_key_derivation_block input_block;
  size_t key_len, input_block_len, final_len;
  const void *key;

  spi = pkt_ctx->psp_cfg.spi;
  input_block_len = (size_t)PSP_KEY_DERIVATION_BLOCK_OCTETS;
  input_block.octets[0] = 0x00;
  input_block.octets[1] = 0x00;
  input_block.octets[2] = 0x00;
  input_block.octets[3] = counter;
  input_block.octets[4] = 0x50;
  input_block.octets[5] = 0x76;
  if (pkt_ctx->psp_cfg.crypto_alg == AES_GCM_128) {
    input_block.octets[6] = 0x30;
    input_block.octets[14] = 0x00;
    input_block.octets[15] = 0x80;
  } else {
    input_block.octets[6] = 0x31;
    input_block.octets[14] = 0x01;
    input_block.octets[15] = 0x00;
  }
  input_block.octets[7] = 0x00;
  input_block.octets[8] = (spi >> 24) & 0xff;
  input_block.octets[9] = (spi >> 16) & 0xff;
  input_block.octets[10] = (spi >> 8) & 0xff;
  input_block.octets[11] = spi & 0xff;
  input_block.octets[12] = 0x00;
  input_block.octets[13] = 0x00;

  if (spi & PSP_SPI_KEY_SELECTOR_BIT)
    key = (const void *)pkt_ctx->psp_cfg.master_key1.octets;
  else
    key = (const void *)pkt_ctx->psp_cfg.master_key0.octets;
  key_len = (size_t)AES_256_KEY_OCTETS;

  ctx = CMAC_CTX_new();
  if (ctx == NULL) {
    fprintf(stderr, "CMAC_CTX_new() failed\n");
    goto err_exit;
  }

  if (!CMAC_Init(ctx, key, key_len, EVP_aes_256_cbc(), NULL)) {
    fprintf(stderr, "CMAC_Init() failed\n");
    goto err_exit;
  }

  if (!CMAC_Update(ctx, (const uint8_t *)input_block.octets, input_block_len)) {
    fprintf(stderr, "CMAC_Update() failed\n");
    goto err_exit;
  }

  if (!CMAC_Final(ctx, derived_key, &final_len)) {
    fprintf(stderr, "CMAC_Final() failed\n");
    goto err_exit;
  }

  CMAC_CTX_free(ctx);
  return SUCCESS_RC;

err_exit:
  if (ctx != NULL) CMAC_CTX_free(ctx);
  return ERR_RC;
}

/*
 * derive psp encryption key
 *
 * returns:
 *   SUCCESS_RC: derived key is returned in packet context structure
 *   ERR_RC:     error deriving key
 */
static rc_t derive_psp_key(struct pkt_context *pkt_ctx) {
    rc_t rc;

    rc = derive_psp_key_128(pkt_ctx, (uint8_t)1, pkt_ctx->key.octets);
    if ((rc != SUCCESS_RC) || (pkt_ctx->psp_cfg.crypto_alg == AES_GCM_128))
        goto end;
    rc = derive_psp_key_128(
        pkt_ctx, (uint8_t)2,
        &pkt_ctx->key.octets[PSP_KEY_DERIVATION_BLOCK_OCTETS]);
end:
    /* printf("Derived Key:\n  "); */
    /* for (int i = 0; i < PSP_KEY_DERIVATION_BLOCK_OCTETS; i++) */
    /*     printf("%02hhx ", pkt_ctx->key.octets[i]); */
    /* if (pkt_ctx->psp_cfg.crypto_alg == AES_GCM_256) { */
    /*     for (int i = 0; i < PSP_KEY_DERIVATION_BLOCK_OCTETS; i++) */
    /*         printf("%02hhx ", */
    /*                pkt_ctx->key.octets[PSP_KEY_DERIVATION_BLOCK_OCTETS + i]); */
    /* } */
    /* printf("\n"); */
    /* fflush(stdout); */
    return rc;
}

/* get next psp initialization vector */
static inline uint64_t get_psp_iv(struct pkt_context *pkt_ctx) {
  uint64_t iv;

  iv = HTONLL(pkt_ctx->next_iv);
  pkt_ctx->next_iv++;
  return iv;
}

/*
 * perform psp encryption
 *
 * the code for this function is based off an example on the
 * OpenSSL website (see https://wiki.openssl.org/images/0/08/Evp-gcm-encrypt.c)
 *
 * parms:
 *   pkt_ctx: ptr to context info for packet
 *   psp: ptr to psp header of packet,
 *        aes_cgm iv is spi concatenated with psp iv,
 *        this is also start of additional authentication data
 *   cleartext_len: length of cleartext to be encrypted in octets
 *   cleartext: ptr to cleartext to encrypt
 *   aad_len: length of additional authentication data
 *   ciphertext: ptr to location where encrypted data is to be returned
 *   icv: ptr to location where integrity check value is to be returned
 *
 * returns:
 *   PKT_ENCRYPTED
 *   PKT_ERR
 * */
static pkt_rc_t psp_encrypt(struct pkt_context *pkt_ctx, struct psp_hdr *psp,
                            uint32_t cleartext_len, uint8_t *cleartext,
                            uint32_t aad_len, uint8_t *ciphertext,
                            struct psp_icv *icv) {
  int rc, len;
  uint8_t *aad = (uint8_t *)psp;
  struct aes_gcm_iv gcm_iv;
  EVP_CIPHER_CTX *ctx = NULL;

  memcpy(gcm_iv.octets, &psp->spi, PSP_SPI_OCTETS);
  memcpy(&gcm_iv.octets[PSP_SPI_OCTETS], &psp->iv, PSP_IV_OCTETS);

  /* create and initialize the cipher context */
  ctx = EVP_CIPHER_CTX_new();
  if (ctx == NULL) {
    fprintf(stderr, "EVP_CIPHER_CTX_new() failed\n");
    goto err_exit;
  }

  /* initialize the encryption operation */
  if (pkt_ctx->psp_cfg.crypto_alg == AES_GCM_128)
    rc = EVP_EncryptInit_ex(ctx, EVP_aes_128_gcm(), NULL, NULL, NULL);
  else
    rc = EVP_EncryptInit_ex(ctx, EVP_aes_256_gcm(), NULL, NULL, NULL);
  if (rc != 1) {
    fprintf(stderr, "EVP_EncryptInit_ex() failed\n");
    goto err_exit;
  }

  /* initialize key and iv */
  if (EVP_EncryptInit_ex(ctx, NULL, NULL, pkt_ctx->key.octets, gcm_iv.octets) !=
      1) {
    fprintf(stderr, "EVP_EncryptInit_ex() failed\n");
    goto err_exit;
  }

  /* provide additional authentication data */
  if (EVP_EncryptUpdate(ctx, NULL, &len, aad, aad_len) != 1) {
    fprintf(stderr, "EVP_EncryptUpdate() failed\n");
    goto err_exit;
  }

  /* do encryption */
  if (EVP_EncryptUpdate(ctx, ciphertext, &len, cleartext, cleartext_len) != 1) {
    fprintf(stderr, "EVP_EncryptUpdate() failed\n");
    goto err_exit;
  }

  /* finalize encryption */
  if (EVP_EncryptFinal_ex(ctx, ciphertext + len, &len) != 1) {
    fprintf(stderr, "EVP_EncryptFinal_ex() failed\n");
    goto err_exit;
  }

  /* get the icv */
  if (EVP_CIPHER_CTX_ctrl(ctx, EVP_CTRL_GCM_GET_TAG, PSP_ICV_OCTETS,
                          icv->octets) != 1) {
    fprintf(stderr, "EVP_CIPHER_CTX_ctrl() failed\n");
    goto err_exit;
  }

  EVP_CIPHER_CTX_free(ctx);
  return PKT_ENCRYPTED;

err_exit:
  if (ctx != NULL) EVP_CIPHER_CTX_free(ctx);
  return PKT_ERR;
}
static pkt_rc_t psp_decrypt(struct pkt_context *pkt_ctx, struct psp_hdr *psp,
                            uint32_t ciphertext_len, uint8_t *ciphertext,
                            uint32_t aad_len, uint8_t *aad, uint8_t *cleartext,
                            struct psp_icv *expected_icv) {
  int rc, len;
  uint8_t psp_ver;
  uint32_t *spi;
  uint64_t *psp_iv;
  struct aes_gcm_iv gcm_iv;
  EVP_CIPHER_CTX *ctx = NULL;

  if (ciphertext_len == 0) return PKT_DECRYPTED;

  /* form aes-gsm iv */
  spi = &pkt_ctx->psp_cfg.spi;
  psp_iv = &psp->iv;
  memcpy(gcm_iv.octets, spi, PSP_SPI_OCTETS);
  memcpy(&gcm_iv.octets[PSP_SPI_OCTETS], psp_iv, PSP_IV_OCTETS);


    pkt_ctx->psp_cfg.spi = ntohl(psp->spi);
  rc = derive_psp_key(pkt_ctx);
  if (rc != SUCCESS_RC) goto err_exit;

  /* create and initialize the cipher context */
  ctx = EVP_CIPHER_CTX_new();
  if (ctx == NULL) {
    fprintf(stderr, "EVP_CIPHER_CTX_new() failed\n");
    goto err_exit;
  }

  /* initialize the decryption operation */
  psp_ver = (psp->s_d_ver_v_1 >> PSP_HDR_VER_SHIFT) & PSP_HDR_VER_MASK;
  if (psp_ver == PSP_VER0)
    rc = EVP_DecryptInit_ex(ctx, EVP_aes_128_gcm(), NULL, NULL, NULL);
  else
    rc = EVP_DecryptInit_ex(ctx, EVP_aes_256_gcm(), NULL, NULL, NULL);
  if (rc != 1) {
    fprintf(stderr, "EVP_DecryptInit_ex() verion failed\n");
    goto err_exit;
  }
  /* initialize key and iv */
  if (EVP_DecryptInit_ex(ctx, NULL, NULL, pkt_ctx->key.octets, gcm_iv.octets) !=
      1) {
    fprintf(stderr, "EVP_DecryptInit_ex() initialize failed\n");
    goto err_exit;
  }

  /* provide additional authentication data */
  if (EVP_DecryptUpdate(ctx, NULL, &len, aad, aad_len) != 1) {
    fprintf(stderr, "EVP_DecryptUpdate() aad failed\n");
    goto err_exit;
  }

  /* do decryption */
  if (EVP_DecryptUpdate(ctx, cleartext, &len, ciphertext, ciphertext_len) !=
      1) {
    fprintf(stderr, "EVP_DecryptUpdate() do failed\n");
    goto err_exit;
  }

  /* set the expected icv */
  if (EVP_CIPHER_CTX_ctrl(ctx, EVP_CTRL_GCM_SET_TAG, PSP_ICV_OCTETS,
                          expected_icv->octets) != 1) {
    fprintf(stderr, "EVP_CIPHER_CTX_ctrl() failed\n");
    goto err_exit;
  }

  /* finalize decryption */
  if (EVP_DecryptFinal_ex(ctx, cleartext + len, &len) < 1) {
    fprintf(stderr, "EVP_DecryptFinal_ex() final failed\n");
    goto err_exit;
  }

  EVP_CIPHER_CTX_free(ctx);
  return PKT_DECRYPTED;

err_exit:
  if (ctx != NULL) EVP_CIPHER_CTX_free(ctx);
  return PKT_ERR;
}

static pkt_rc_t parse_packet(struct pkt_context *pkt_ctx) {
    struct psp_hdr *psp;
    struct psp_icv *icv;
    uint8_t  *out_pkt, *after_base_psp_hdr, *after_ext_psp_hdr,
    *cipher_start, *aad, *out_cleartext;
    uint32_t 
    psp_hdr_len, hdr_ext_len, psp_trailer_len, psp_payload_len, psp_ver,
    crypt_off, crypt_off_after_ext, encrypted_ext_len,
    cipher_len, aad_len, cleartext_copy_len;
    crypto_alg_t crypto_alg;
    pkt_rc_t pkt_rc;

    /* parse input packet */
    psp = (struct psp_hdr *)pkt_ctx->in_pkt;
    psp_hdr_len = sizeof(struct psp_hdr);
    psp_trailer_len = sizeof(struct psp_trailer);
    /* including psp hdr ext in psp_payload_len */
    psp_payload_len = pkt_ctx->psp_payload_len - (psp_hdr_len + psp_trailer_len);
    /* printf("psp_payload_len:%d strlen:%d \n",psp_payload_len, pkt_ctx->psp_payload_len); */
    hdr_ext_len = (psp->hdr_ext_len - PSP_HDR_EXT_LEN_MIN) * PSP_HDR_EXT_LEN_UNITS;

    /* pkt_ctx->psp_cfg.spi = ntohl(psp->spi); */
    pkt_ctx->psp_cfg.spi = psp->spi;
    
    /* psp->iv = NTOHLL(psp->iv); */
    if (hdr_ext_len > psp_payload_len) {
        fprintf(stderr, "invalid packet, hdr_ext_len exceeds packet size\n");
        return PKT_ERR;
    }

    crypt_off = (psp->crypt_off & PSP_CRYPT_OFFSET_MASK) * PSP_CRYPT_OFFSET_UNITS;
    if (crypt_off > psp_payload_len) {
        fprintf(stderr, "invalid packet, crypt offset too big, %u\n", crypt_off);
        return PKT_ERR;
    }

    psp_ver = (psp->s_d_ver_v_1 >> PSP_HDR_VER_SHIFT) & PSP_HDR_VER_MASK;
    switch (psp_ver) {
        case PSP_VER0:
            crypto_alg = AES_GCM_128;
            break;
        case PSP_VER1:
            crypto_alg = AES_GCM_256;
            break;
        default:
            fprintf(stderr, "invalid packet, unsupported PSP version, %u\n", psp_ver);
            return PKT_ERR;
    }
    pkt_ctx->psp_cfg.crypto_alg = crypto_alg;
    if ((psp->s_d_ver_v_1 & PSP_HDR_ALWAYS_1) != PSP_HDR_ALWAYS_1) {
        fprintf(stderr,
                "invalid packet, invalid word 0 in PSP header, "
                "0x%08x\n",
                *((uint32_t *)psp));
        return PKT_ERR;
    }

    /*
   * build the cleartext packet
   *   - copy the ethernet header of the input packet
   *   - if transport mode
   *   -   insert ip header based on ip header of input packet
   *   - copy data from input packet starting after psp header
   *   - check icv and decrypt data
   */
    out_pkt = pkt_ctx->out_pkt;

    after_base_psp_hdr = ((uint8_t *)psp) + psp_hdr_len;
    after_ext_psp_hdr = after_base_psp_hdr + hdr_ext_len;
    if (crypt_off >= hdr_ext_len) {
        encrypted_ext_len = 0;
        crypt_off_after_ext = crypt_off - hdr_ext_len;
    } else {
        encrypted_ext_len = hdr_ext_len - crypt_off;
        crypt_off_after_ext = 0;
    }
    memcpy(out_pkt, after_ext_psp_hdr, crypt_off_after_ext);
    out_pkt += crypt_off_after_ext;

    cipher_start = after_base_psp_hdr + crypt_off;
    cipher_len = psp_payload_len - crypt_off;
    aad = (uint8_t *)psp;
    aad_len = psp_hdr_len + crypt_off;
    out_cleartext = pkt_ctx->scratch_buf;
    icv = (struct psp_icv *)(cipher_start + cipher_len);
    pkt_rc = psp_decrypt(pkt_ctx, psp, cipher_len, cipher_start, aad_len, aad,
                         out_cleartext, icv);
    if (pkt_rc != PKT_DECRYPTED) return pkt_rc;
    cleartext_copy_len = cipher_len - encrypted_ext_len;
    memcpy(out_pkt, out_cleartext + encrypted_ext_len, cleartext_copy_len);
    /* printf("out_pkt:%s cleartext_copy_len:%d\n",out_pkt,cleartext_copy_len); */

    /* printf("after decrypt:"); */
    /* for(int i = 0; i < crypt_off ; i++) */
    /*     printf("%02x ", pkt_ctx->in_pkt[i + psp_hdr_len]); */
    /* for(int i = 0; i < cleartext_copy_len; i++) */
    /*     printf("%02x ", out_pkt[i]); */
    /* fflush(stdout); */
    out_pkt += cleartext_copy_len;

    return PKT_DECRYPTED;
}

/* perform transport mode psp encapsulation */
static pkt_rc_t transport_encap(struct pkt_context *pkt_ctx) {
    struct udp_hdr *psp_udp;
    struct psp_hdr *psp;
    struct psp_icv *out_icv;
    uint8_t *in_pkt, *out_pkt, *out_l4, *buf, *in_encrypt,
    *out_encrypt, psp_ver;
    uint16_t *in_l4;
    uint32_t udp_hdr_len, vc_octets, psp_encap_octets, base_psp_hdr_len, psp_hdr_len,
    psp_payload_len, crypt_off, crypt_off_after_ext, encrypt_len, aad_len;
    uint64_t *vc;
    pkt_rc_t pkt_rc;

    in_pkt = pkt_ctx->in_pkt;
    psp_payload_len = SEND_NUMBER;
    crypt_off = pkt_ctx->psp_cfg.transport_crypt_off * PSP_CRYPT_OFFSET_UNITS;

    if (crypt_off > psp_payload_len) {
        fprintf(stderr, "skipping packet, crypt offset too big\n");
        return PKT_SKIPPED;
    }

    /* printf("in_pkt:%s len:%d crypt_off:%d\n",in_pkt, psp_payload_len, crypt_off); */
    out_pkt = pkt_ctx->out_pkt;

    if (pkt_ctx->psp_cfg.include_vc)
        vc_octets = PSP_HDR_VC_OCTETS;
    else
        vc_octets = 0;

    if (crypt_off > vc_octets) {
        crypt_off_after_ext = crypt_off - vc_octets;
    } else {
        crypt_off_after_ext = 0;
    }

    base_psp_hdr_len = sizeof(struct psp_hdr);
    psp_hdr_len = base_psp_hdr_len + vc_octets;
    psp_encap_octets = PSP_TRANSPORT_ENCAP_OCTETS + vc_octets;

    psp_udp = (struct udp_hdr *)out_pkt;
    in_l4 = (uint16_t *)in_pkt;
    psp_udp->sport = htons(6666);
    psp_udp->dport = htons(UDP_PORT_PSP);
    psp_udp->len = htons(psp_payload_len + psp_encap_octets);
    psp_udp->csum = 0;
    udp_hdr_len = sizeof(struct udp_hdr);

    psp = (struct psp_hdr *)(((uint8_t *)psp_udp) + udp_hdr_len);
    psp->next_hdr = IP_PROTO_UDP;
    if (pkt_ctx->psp_cfg.crypto_alg == AES_GCM_128)
        psp_ver = PSP_VER0;
    else
        psp_ver = PSP_VER1;
    if (pkt_ctx->psp_cfg.include_vc) {
        psp->hdr_ext_len = PSP_HDR_EXT_LEN_WITH_VC;
        psp->s_d_ver_v_1 =
            (psp_ver << PSP_HDR_VER_SHIFT) | PSP_HDR_FLAG_V | PSP_HDR_ALWAYS_1;
        vc = (uint64_t *)(((uint8_t *)psp) + base_psp_hdr_len);
        *vc = 0;
    } else {
        psp->hdr_ext_len = PSP_HDR_EXT_LEN_MIN;
        psp->s_d_ver_v_1 = (psp_ver << PSP_HDR_VER_SHIFT) | PSP_HDR_ALWAYS_1;
    }
    psp->crypt_off = pkt_ctx->psp_cfg.transport_crypt_off;
    psp->spi = htonl(pkt_ctx->psp_cfg.spi);
    psp->iv = get_psp_iv(pkt_ctx);

    /* for(int i = 0; i < psp_payload_len + psp_encap_octets; i++) { */
    /*     printf("%02X ",out_pkt[i]); */
    /* } */
    out_l4 = ((uint8_t *)psp) + psp_hdr_len;
    memcpy(out_l4, in_l4, crypt_off_after_ext); //offset n bytes to encrypt
    /* printf("out_l4:%s %d \n",out_l4, crypt_off_after_ext); */

    /* build buffer for icv/encryption computation */
    buf = pkt_ctx->scratch_buf;
    memcpy(buf, psp, psp_hdr_len);
    memcpy(buf + psp_hdr_len, in_l4, psp_payload_len);

    /* compute icv and do encryption */
    in_encrypt = buf + base_psp_hdr_len + crypt_off;
    out_encrypt = ((uint8_t *)psp) + base_psp_hdr_len + crypt_off;
    encrypt_len = vc_octets + psp_payload_len - crypt_off;
    aad_len = base_psp_hdr_len + crypt_off;
    out_icv = (struct psp_icv *)(out_encrypt + encrypt_len);
    pkt_rc = psp_encrypt(pkt_ctx, psp, encrypt_len, in_encrypt, aad_len,
                         out_encrypt, out_icv);
    if (pkt_rc != PKT_ENCRYPTED) return pkt_rc;

    return PKT_ENCRYPTED;
}

/*
 * get psp configuration by:
 *   - reading from configuration file,
 *   - parsing the configuration data, and
 *   - saving the results in the packet context structure
 */
static rc_t get_psp_cfg(char *cfg_file, struct pkt_context *pkt_ctx, int mode) {
    int i;
    FILE *fp;
    char string[16];

    fp = fopen(cfg_file, "r");
    if (fp == NULL) {
        perror("fopen() failed for psp_cfg_file");
        goto err_exit;
    }

    for (i = 0; i < PSP_MASTER_KEY_OCTETS; i++) {
        if (fscanf(fp, "%hhx", &pkt_ctx->psp_cfg.master_key0.octets[i]) != 1) {
            fprintf(stderr, "read of master key 0 from psp_cfg_file failed\n");
            goto err_exit;
        }
    }

    printf("Master Key 0:\n  ");
    for (i = 0; i < PSP_MASTER_KEY_OCTETS; i++)
        printf("%02hhx ", pkt_ctx->psp_cfg.master_key0.octets[i]);
    printf("\n");
    fflush(stdout);

    for (i = 0; i < PSP_MASTER_KEY_OCTETS; i++) {
        if (fscanf(fp, "%hhx", &pkt_ctx->psp_cfg.master_key1.octets[i]) != 1) {
            fprintf(stderr, "read of master key 1 from psp_cfg_file failed\n");
            goto err_exit;
        }
    }

    printf("Master Key 1:\n  ");
    for (i = 0; i < PSP_MASTER_KEY_OCTETS; i++)
        printf("%02hhx ", pkt_ctx->psp_cfg.master_key1.octets[i]);
    printf("\n");
    fflush(stdout);

    if (mode == PSP_MODE_DECRYPT) {
        goto normal_exit;
    }

    if (fscanf(fp, "%x", &pkt_ctx->psp_cfg.spi) != 1) {
        fprintf(stderr, "read of spi from psp_cfg_file failed\n");
        goto err_exit;
    }

    printf("SPI: %08x\n", pkt_ctx->psp_cfg.spi);
    fflush(stdout);

    if (fscanf(fp, "%15s", string) != 1) {
        fprintf(stderr, "read of psp encap mode from psp_cfg_file failed\n");
        goto err_exit;
    }
    if (strcmp(string, "transport") == 0) {
        pkt_ctx->psp_cfg.psp_encap = PSP_TRANSPORT;
    } else if (strcmp(string, "tunnel") == 0) {
        pkt_ctx->psp_cfg.psp_encap = PSP_TUNNEL;
    } else {
        fprintf(stderr, "invalid psp encap mode in psp_cfg_file\n");
        goto err_exit;
    }

    printf("Encap Mode: %s\n", string);
    fflush(stdout);

    if (fscanf(fp, "%15s", string) != 1) {
        fprintf(stderr, "read of crypto algorithm from psp_cfg_file failed\n");
        goto err_exit;
    }
    if (strcmp(string, "aes-gcm-128") == 0) {
        pkt_ctx->psp_cfg.crypto_alg = AES_GCM_128;
    } else if (strcmp(string, "aes-gcm-256") == 0) {
        pkt_ctx->psp_cfg.crypto_alg = AES_GCM_256;
    } else {
        fprintf(stderr, "invalid crypto algotithm in psp_cfg_file\n");
        goto err_exit;
    }

    printf("Crypto Alg: %s\n", string);
    fflush(stdout);

    if (fscanf(fp, "%hhu", &pkt_ctx->psp_cfg.transport_crypt_off) != 1) {
        fprintf(stderr,
                "read of transport crypt offset from psp_cfg_file failed\n");
        goto err_exit;
    }
    if (pkt_ctx->psp_cfg.transport_crypt_off > PSP_CRYPT_OFFSET_MAX) {
        fprintf(stderr,
                "invalid transport crypt offset in psp_cfg_file: "
                "value = %hhu, max value = %d\n",
                pkt_ctx->psp_cfg.transport_crypt_off, PSP_CRYPT_OFFSET_MAX);
        goto err_exit;
    }

    printf("Transport Mode Crypt Offset:   %hhu (%u bytes)\n",
           pkt_ctx->psp_cfg.transport_crypt_off,
           pkt_ctx->psp_cfg.transport_crypt_off * PSP_CRYPT_OFFSET_UNITS);
    fflush(stdout);

    if (fscanf(fp, "%hhu", &pkt_ctx->psp_cfg.ipv4_tunnel_crypt_off) != 1) {
        fprintf(stderr,
                "read of ipv4 tunnel crypt offset from psp_cfg_file failed\n");
        goto err_exit;
    }
    if (pkt_ctx->psp_cfg.ipv4_tunnel_crypt_off > PSP_CRYPT_OFFSET_MAX) {
        fprintf(stderr,
                "invalid ipv4 tunnel crypt offset in psp_cfg_file: "
                "value = %hhu, max value = %d\n",
                pkt_ctx->psp_cfg.ipv4_tunnel_crypt_off, PSP_CRYPT_OFFSET_MAX);
        goto err_exit;
    }

    printf("Tunnel Mode IPv4 Crypt Offset: %hhu (%u bytes)\n",
           pkt_ctx->psp_cfg.ipv4_tunnel_crypt_off,
           pkt_ctx->psp_cfg.ipv4_tunnel_crypt_off * PSP_CRYPT_OFFSET_UNITS);
    fflush(stdout);

    if (fscanf(fp, "%hhu", &pkt_ctx->psp_cfg.ipv6_tunnel_crypt_off) != 1) {
        fprintf(stderr,
                "read of ipv6 tunnel crypt offset from psp_cfg_file failed\n");
        goto err_exit;
    }
    if (pkt_ctx->psp_cfg.ipv6_tunnel_crypt_off > PSP_CRYPT_OFFSET_MAX) {
        fprintf(stderr,
                "invalid ipv6 tunnel crypt offset in psp_cfg_file: "
                "value = %hhu, max value = %d\n",
                pkt_ctx->psp_cfg.ipv6_tunnel_crypt_off, PSP_CRYPT_OFFSET_MAX);
        goto err_exit;
    }

    printf("Tunnel Mode IPv6 Crypt Offset: %hhu (%u bytes)\n",
           pkt_ctx->psp_cfg.ipv6_tunnel_crypt_off,
           pkt_ctx->psp_cfg.ipv6_tunnel_crypt_off * PSP_CRYPT_OFFSET_UNITS);
    fflush(stdout);

    if (fscanf(fp, "%15s", string) != 1) {
        fprintf(stderr, "read of vc string from psp_cfg_file failed\n");
        goto err_exit;
    }
    if (strcmp(string, "vc") == 0) {
        pkt_ctx->psp_cfg.include_vc = true;
    } else if (strcmp(string, "no-vc") == 0) {
        pkt_ctx->psp_cfg.include_vc = false;
    } else {
        fprintf(stderr, "invalid vc string in psp_cfg_file\n");
        goto err_exit;
    }

    printf("VC Mode: %s\n", string);
    fflush(stdout);

normal_exit:
    fclose(fp);
    return SUCCESS_RC;

err_exit:
    if (fp != NULL) fclose(fp);
    return ERR_RC;
}
#endif /* _PSP_H_*/
